COMMENT ⊗   VALID 00002 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	REQUIRE "MOVE.DEF[PNT,HE]" SOURCE_FILE
C00004 ENDMK
C⊗;
REQUIRE "MOVE.DEF[PNT,HE]" SOURCE_FILE;

EXTERNAL INTEGER PROCEDURE TLKEF5(REAL ARRAY TNS,ANGLES);
	! fills TNS with transpose(<arm trans>), ANGLES with
	  joint angles (ANGLES[7]=hand) in degrees & inches.
	  Returns 0 if all ok, otherwise returns ERROR CODE;


PRESET_WITH
	"ELF IS BUSY, SOMEONE IS USING IT",
	"ELF STATUS WORD INDICATES ERROR CONDITION",
	"UNABLE TO ZERO MASTER NUMBER IN ELF",
	"ELF NOT TRANSFERRING ARM POSITION",
	"BAD READ FROM ELF DURING POSITION DATA TRANSFER",
	"IOPDLOV WHEN TRYING TO SAVE CHANNEL 1 FOR ELF, TELL MSM ABOUT IT";
INTERNAL STRING ARRAY ARMERR[1:6];

INTERNAL REAL ARRAY BANGLE,YANGLE[1:7];

INTERNAL PROCEDURE READ_ARM(INTEGER ARM);
	BEGIN
	INTEGER I,J;
	OWN REAL ARRAY BESTNS[1:4,1:3], A[1:5,1:4],BESANGLES[1:7];
	IF ARM=BLUE
	THEN BEGIN
		IF (I←TLKEF5(BESTNS,BESANGLES))>0 THEN 
			ABORT1(ARMERR[I],NULL);
		ARRCLR(A);
		FOR I← 1 STEP 1 UNTIL 3
			DO FOR J←1 STEP 1 UNTIL 4
				DO A[I,J]←BESTNS[J,I];
		A[4,4]←1.0;
		SETABS(F_BARM,A);
		FOR I←1 STEP 1 UNTIL 7 DO BANGLE[I]←BESANGLES[I];
		SCALAR:VALUE[S_BHAND] ←BESANGLES[7];	! RETURNS BLUE HAND OPENING;
	     END
	ELSE ABORT1("CANT READ YELLOW ARM YET",NULL);

	END;

PROCEDURE READ_BLUE;
	READ_ARM(BLUE);